import numpy as np
import math,json
import open3d as o3d
import copy
import numpy as np

import open3d as o3d
'''
# 如果点多，用ball等表面重建，如果点少，convex hull，然后sample points poisson,体素可以查看是否内部，所以做地图比较好。
1 点云到体积的处理套路：如果稀疏，走convex hull路线，然后poisson采样可以加密度，如果很多很乱，走ball pivot，我管他叫滚雪球算法。
2 算mesh的体积有个数学技巧，先把中中心找到，然后把表面的三角形和中心连接形成三棱锥，然后把所有的三棱锥加起来。这个是我今天灵机一动想到的，简单有效，远比stackoverflow的voxel有效。

此外voxel体素有一个很大的用处，因为他很容易check一个三维point是否在体内，所以用来做3维地图特备合适。
'''
def open_glb():
    mesh = o3d.io.read_triangle_mesh("./box.obj")
    o3d.visualization.draw_geometries([mesh])

def create():
    x = np.linspace(-3, 3, 401)
    mesh_x, mesh_y = np.meshgrid(x, x)
    z = np.sinc((np.power(mesh_x, 2) + np.power(mesh_y, 2)))
    z_norm = (z - z.min()) / (z.max() - z.min())
    xyz = np.zeros((np.size(mesh_x), 3))
    xyz[:, 0] = np.reshape(mesh_x, -1)
    xyz[:, 1] = np.reshape(mesh_y, -1)
    xyz[:, 2] = np.reshape(z_norm, -1)
    print('xyz')
    print(xyz)
    l=[[0,0,0],[1,0,0],[0,1,0],[0,1,0],[0,0,1],[0,0,0]]
    xyz=np.array(l)
    
    # Pass xyz to Open3D.o3d.geometry.PointCloud and visualize
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(xyz)
    # o3d.visualization.draw_geometries([pcd])
    o3d.io.write_point_cloud("./sync.ply", pcd)
def ball_surface(f="./sync.ply"):
    pcd_load = o3d.io.read_point_cloud(f)
    pcd_load.estimate_normals()
    distances = pcd_load.compute_nearest_neighbor_distance()
    avg_dist = np.mean(distances)
    radius = 3 * avg_dist
    print(radius)
    
    # ball
    bpa_mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting(pcd_load, o3d.utility.DoubleVector(
        [radius, radius * 2]))
    o3d.visualization.draw_geometries([bpa_mesh], mesh_show_back_face=True)
def poisson():
    pcd_load = o3d.io.read_point_cloud("./sync.ply")
    pcd_load.estimate_normals()
    
    # poisson
    poisson_mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson(pcd_load, depth=8, width=0, scale=1.1, linear_fit=False)[
        0]
    bbox = pcd_load.get_axis_aligned_bounding_box()
    p_mesh_crop = poisson_mesh.crop(bbox)
    o3d.visualization.draw_geometries([poisson_mesh], mesh_show_back_face=True)
def convex_open3d():
    l = [[0, 0, 0], [1, 0, 0], [0, 1, 0], [1,1,0], [0, 0, 1],[1,1,1],[1,0,1],[0,1,1] ]
    xyz = np.array(l)
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(xyz)
    
    hull, _ = pcd.compute_convex_hull()
    # hull_ls = o3d.geometry.LineSet.create_from_triangle_mesh(hull)
    # hull_ls.paint_uniform_color((1, 0, 0))
    # o3d.visualization.draw_geometries([pcd, hull_ls])
    pcd2 =hull.sample_points_poisson_disk(2500)
    o3d.visualization.draw_geometries([pcd2])
    # o3d.io.write_point_cloud("./sync.ply", hull_ls)
    return hull

###############################################################
def volume_from_convex_hull(mesh):
    center = mesh.get_center()
    vers = np.asarray(mesh.vertices)
    tris = np.asarray(mesh.triangles)
    sum = 0
    for i in range(tris.shape[0]):
        a, b, c = tris[i]
        ae = vers[a] - center
        be = vers[b] - center
        ce = vers[c] - center
        cross = np.cross(ae, be)
        s = np.linalg.norm(cross) / 2
        h = np.dot(cross, ce) / np.linalg.norm(cross)
        vol = np.abs(s * h / 3)
        sum = vol + sum
    print("my sum",sum)
    return sum

if __name__ == "__main__":
    open_glb()
    # generate some neat n times 3 matrix using a variant of sync function
    # Load saved point cloud and visualize it
    # m=convex_open3d()
    # volume_from_convex_hull(m)


    